//
//  main.cpp



#include <math.h>
#include <stdio.h>
#include<algorithm>
#include <iostream>
#include <string>
#include "opencv2/core/core.hpp"
#include "opencv2/core/utility.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui/highgui.hpp"

#define INF1 9999
using namespace std;
using namespace cv;
float dist(float x1, float y1 ,float x2, float y2)
{
    double dist = sqrt((x1- x2)*(x1-x2) + (y1 - y2)*(y1 - y2));
    return dist;
}
float count_slop(float x1, float y1, float x2, float y2)
{
    float slop ;
    if(fabs(x1 - x2) < 0.0001)
        slop = INF1;
    else
        slop = (y2 - y1) / ( x2 - x1);
    
    return slop;
}
float count_b(float x1, float y1, float slop)
{
    float b ;
    if(slop == INF1)
    {
        
        b = x1;
        return b;
    }
    if( fabs(slop) > 9)
        b =x1;
    else
        b = y1 - slop * x1;
    return b;
}

float count_r(float x1, float y1, float slop)
{
    float r;
    float theta = atan(slop);
    r = x1 * cos(theta) + y1 * sin(theta);
    return r;
}

int draw_line(Mat drwa, vector<Vec4f> lines_std)
{
    vector<Vec4f> line_final;
    
    for(int i = 0; i<lines_std.size(); i++)
    {
        
        line_final.push_back(lines_std[i]);
    }
    for(int i=0; i<line_final.size()
        ; i++)
    {
        
        
        line(drwa, Point(line_final[i][0],line_final[i][1]),Point(line_final[i][2],line_final[i][3]), Scalar(0,0,255));
        cout << i << endl;
        imshow("Standard refinement1", drwa);
        imwrite("/Users/yuefei/Desktop/连线后的图像/dst1.3.jpeg", drwa);
        waitKey();
        
    }
    return 0;
}
int draw_line1(Mat drwa, vector<Vec4f> lines_std[100], int accum)
{
    
    for(int i=0; i<accum; i++)
    {
        int a = i * 64;
        for(int j=0;j<lines_std[i].size(); j++)
        {
            line(drwa, Point(lines_std[i][j][0],lines_std[i][j][1]),Point(lines_std[i][j][2],lines_std[i][j][3]), Scalar(0,0,255));
            
            char s[10];
             sprintf(s, "%d",j);
             Point l;
             int font_face = cv::FONT_HERSHEY_COMPLEX;
             l.x = lines_std[i][j][2]  ;
             l.y = lines_std[i][j][3] ;
             putText( drwa, s, l, font_face,0.4,Scalar( a, a, a ),1);
             //cout << i << endl;*/
            
        }
        imshow("S1", drwa);
        imwrite("/Users/yuefei/Desktop/8.png", drwa);
        waitKey();
    }
    imshow("S1", drwa);
    //imwrite("/Users/yuefei/Desktop/6.png", drwa);
    waitKey();
    
    return 0;
}
int draw_line2(Mat drwa, vector<Vec6f> lines_std[100], int accum)
{
    
    
    for(int i=0; i<accum; i++)
    {
        int a =0;
        if(i == 0 ) a = 128;
        if(i == 1 ) a = 64;
        if(i ==2 ) a = 192;
        if(i == 3) a = 255;
        for (int j=0; j < lines_std[i].size(); j++)
        {
            cout << lines_std[i][j] << endl;
            line(drwa, Point(lines_std[i][j][0],lines_std[i][j][1]),Point(lines_std[i][j][2],lines_std[i][j][3]), Scalar(0,0,255));
            float slop ;
            slop = count_slop(lines_std[i][j][0],lines_std[i][j][1], lines_std[i][j][2],lines_std[i][j][3]) ;
            //cout << slop << endl;
            /* char s[10];
             sprintf(s, "%d",j);
             Point l;
             int font_face = cv::FONT_HERSHEY_COMPLEX;
             l.x = lines_std[i][j][2]  ;
             l.y = lines_std[i][j][3] ;
             putText( drwa, s, l, font_face,0.4,Scalar( 255,a,a),1);*/
            //imshow("draw_line2", drwa);
            //waitKey();
            // imwrite("/Users/yuefei/Desktop/9.png", drwa);
            //waitKey();
        }
        imshow("draw_line2", drwa);
        imwrite("/Users/yuefei/Desktop/9.png", drwa);
    }
    
    waitKey();
    return 0;
}
int Is_equal ( Vec6f line1, Vec6f line2)
{
    int flag=0;
    if(((line1[0] - line2[0])==0)&&((line1[0] - line2[0])==0)&& ((line1[0] - line2[0])==0)&&((line1[0] - line2[0])==0))
    {
        
        flag =1;
        return flag;
    }
    else
    {
        
        flag = 0;
        return flag;
    }
    
}

int Is_equal1 ( Vec4f line1, Vec4f line2)
{
    int flag=0;
    if(((line1[0] - line2[0])==0)&&((line1[0] - line2[0])==0)&& ((line1[0] - line2[0])==0)&&((line1[0] - line2[0])==0))
    {
        
        flag =1;
        return flag;
    }
    else
    {
        
        flag = 0;
        return flag;
    }
    
}
int identify(float y1, float y2)
{
    int flag =0;
    
    if((y1 > 0) &&(y2 > 0) ) flag = 1;
    if((y1 < 0) &&(y2 < 0) ) flag = 1;
    if((y1 > 0) &&(y2 < 0) ) flag = -1;
    if((y1 < 0) &&(y2 > 0) ) flag = -1;
    return flag;
}

float find_max(float dist1, float dist2, float dist3, float dist4)
{
    float max;
    if(dist1 < dist2) max = dist2;
    else max = dist1;
    if(max < dist3) max = dist3;
    if(max < dist4) max = dist4;
    
    return max;
}

float find_min(float dist1, float dist2, float dist3, float dist4)
{
    float min;
    if(dist1 < dist2) min = dist1;
    else min = dist2;
    if(min > dist3) min = dist3;
    if(min > dist4) min = dist4;
    
    return min;
}


int Connect_line(Vec4f &l2, Vec4f l1)
{
    Vec4f l;
    l[0] = l2[0];
    l[1] = l2[1];
    l[2] = l2[2];
    l[3] = l2[3];
    double dist1,dist2,dist4,dist3;
    double length1, length2;
    length1 = dist(l2[0],l2[1],l2[2],l2[3]);
    dist1 = dist(l[0], l[1], l1[0], l1[1]);
    dist2 = dist(l[0], l[1], l1[2], l1[3]);
    dist3 = dist(l[2], l[3], l1[0], l1[1]);
    dist4 = dist(l[2], l[3], l1[2], l1[3]);
    double max;
    max = find_max(dist1,dist2,dist3,dist4);
    
    if(max == dist1)
    {
        l[0] = l[0];
        l[1] = l[1];
        l[2] = l1[0];
        l[3] = l1[1];
        
    }
    if(max == dist2)
    {
        l[0] = l[0];
        l[1] = l[1];
        l[2] = l1[2];
        l[3] = l1[3];
        
    }
    if(max == dist3)
    {
        l[0] = l[2];
        l[1] = l[3];
        l[2] = l1[0];
        l[3] = l1[1];
    }
    if(max == dist4)
    {
        l[0] = l[2];
        l[1] = l[3];
        l[2] = l1[2];
        l[3] = l1[3];
    }
    length2 = dist(l[0],l[1],l[2],l[3]);
    if(length2 > length1)
    {
        
        l2[0] = l[0];
        l2[1] = l[1];
        l2[2] = l[2];
        l2[3] = l[3];
        
    }
    
    return 1;
}
int Delete_line(vector<Vec4f> lines_std2[100],vector<Vec4f> lines_std3[100],int &count,Mat draw)
{
    
    Vec4f l,med;
    int accum=0;
    for(int i = 0; i < count; i++)
    {
        int max = 0;
        int index=-1;
        int flag = 0;
        if(lines_std3[accum].size() < 1)
        {
            for(int j = 0; j < lines_std2[i].size(); j++)
            {
                
                float length;
                length = dist(lines_std2[i][j][0],lines_std2[i][j][1],lines_std2[i][j][2],lines_std2[i][j][3]);
                if(length > max)
                {
                    max = length;
                    index = j;
                }
            }
            
            l[0] = lines_std2[i][index][0];
            l[1] = lines_std2[i][index][1];
            l[2] = lines_std2[i][index][2];
            l[3] = lines_std2[i][index][3];
            lines_std3[accum].push_back(l);
            lines_std2[i].erase(lines_std2[i].begin() + index);
        }
        
        for(int k = 0; k < lines_std2[i].size(); k++)
        {
            double dist1,dist2,dist4,dist3;
            dist1 = dist(lines_std2[i][k][0], lines_std2[i][k][1], l[0],l[1]);
            dist2 = dist(lines_std2[i][k][0], lines_std2[i][k][1], l[2],l[3]);
            dist3 = dist(lines_std2[i][k][2], lines_std2[i][k][3], l[0],l[1]);
            dist4 = dist(lines_std2[i][k][2], lines_std2[i][k][3], l[2],l[3]);
            float min = find_min(dist1, dist2, dist3, dist4);
            if(min < 20)
            {
//                cout<< " ******* " << endl;
//                cout << lines_std2[i][k] << endl;
//                cout << "连线前" << l <<endl;
//                cout << l << endl;
//                cout << lines_std2[i][k] << endl;
//                cout << "******" << endl;
                Connect_line(l, lines_std2[i][k]);
                
                
                lines_std2[i].erase(lines_std2[i].begin() + k);
                flag = 1;
            }
            
        }
        if(flag == 1)
            i = i - 1;
        else
        {
            lines_std3[accum][0][0] = l[0];
            lines_std3[accum][0][1] = l[1];
            lines_std3[accum][0][2] = l[2];
            lines_std3[accum][0][3] = l[3];
            
            //cout << " jieguo " <<l << endl;
            //line(draw, Point(lines_std3[accum][0][0],lines_std3[accum][0][1]),Point(lines_std3[a][accum][2],lines_std3[accum][0][3]), Scalar(0,0,255));
            accum++;
            //imshow("draw", draw);
            //waitKey();
            
        }
    }
    count = accum;
    //    for(int i = 0; i < accum;i++)
    //    {
    //        line(draw, Point(lines_std3[i][0][0],lines_std3[i][0][1]),Point(lines_std3[i][0][2],lines_std3[i][0][3]), Scalar(0,0,255));
    //
    //
    //    }
    //imshow("draw", draw);
    // waitKey();
    return 1;
}

int Line_detect(Mat src, vector<Rect> Image_rect,vector<vec4f> line_detct[50])
{
    
    
    for(int i=0;i<Image_rect.size();i++)
    {
        
        Mat dst;
        Mat image = src(Rect(i));
        cvtColor( image, dst, CV_BGR2GRAY );
        blur( dst, dst, Size(3,3) );
        Canny(dst,dst,20,60);
        Ptr<LineSegmentDetector> ls = createLineSegmentDetector(LSD_REFINE_ADV);//或者两种LSD算法，这边用的是ADV的
        vector<Vec4f> lines_std,lines_improve1,lines_std1,lines_std2[100],line_dect,lines_std3[100];
        vector<Vec6f> line_hough,lines_hough1[100],lines_hough2[100],line_hough3;
        Vec4f l,lmd;
        Vec6f med,med1;
        vector<float> Length;
        Mat draw(image);
        vector<float> a,b,c;
        ls->detect(dst, lines_std,a,b,c);//这里把检测到的直线线段都存入了lines_std中，4个float的值，分别为起止点的坐标
        for(int i=0; i < lines_std.size();i++)
        {
            if((lines_std[i][0]>4)&&(lines_std[i][1]>4))
                lines_std1.push_back(lines_std[i]);
        }
        
       
        for(int i=0; i<lines_std1.size();i++)
        {
            med[0] = lines_std1[i][0];
            med[1] = lines_std1[i][1];
            med[2] = lines_std1[i][2];
            med[3] = lines_std1[i][3];
            med[4] = count_slop(med[0], med[1], med[2], med[3]);
            med[5] = count_b(med[0], med[1], med[4]);;
            line_hough.push_back(med);
            
        }
        double slop1,slop2,identify1;
        
        int index = 0;
        
        for(int i=0; i<line_hough.size();i++)
        {
            int flag = 0;
            if(lines_hough1[i].size() < 1)
                lines_hough1[i].push_back(line_hough[i]);
            
            if(index < lines_hough1[i].size())
            {
                med[0] = lines_hough1[i][index][0];
                med[1] = lines_hough1[i][index][1];
                med[2] = lines_hough1[i][index][2];
                med[3] = lines_hough1[i][index][3];
                med[4] = lines_hough1[i][index][4];
                med[5] = 0;
                slop1 = med[4];
                for(int j=0; j<line_hough.size(); j++)
                {
                    if(Is_equal(med, line_hough[j]) == 0)
                    {
                        
                        slop2 = line_hough[j][4];
                        double theta1,theta2;
                        theta1 = atan(slop1) * 180 / CV_PI;
                        theta2 = atan(slop2) * 180 / CV_PI;
                        identify1 = identify(theta1,theta2);
                        
                        if((fabs(theta1 - theta2) < 5)&&( identify1>0))
                        {
                            float b1,b2;
                            b1 =count_b(med[0],med[1],slop1);
                            b2 = count_b(line_hough[j][0],line_hough[j][1],slop2);
                            
                            if(fabs(b1 - b2) < 35)
                            {
                                lines_hough1[i].push_back(line_hough[j]);
                                line_hough.erase(line_hough.begin() + j);
                                
                                flag = 1;
                            }
                            
                        }
                    }
                }
            }
            if(flag == 1)
            {
                i = i - 1;
                index++;
            }
            if(flag == 0)
            {
                if(index < lines_hough1[i].size())
                {
                    i = i -1;
                    index++;
                }
                else
                    index = 0;
            }
        }
        
        int count =0;
        for(int i=0; i< lines_std1.size(); i++)
        {
            if(lines_hough1[i].size() >3)
            {
                for(int j=0; j < lines_hough1[i].size(); j++)
                {
                    lines_hough2[count].push_back(lines_hough1[i][j]);
                }
                count++;
            }
        }
        
        int count1 = 0;
        for(int i = 0; i < count; i++)
        {
            int flag = 0;
            for(int j = 0; j < lines_hough2[i].size() ; j++)
            {
                for(int k = 0; k < lines_hough2[i].size(); k++)
                {
                    if(Is_equal(lines_hough2[i][j], lines_hough2[i][k]) == 0)
                    {
                        //cout << j << " " << k << endl;
                        //cout <<Is_equal(lines_hough2[i][j], line_hough[i][k]) << endl;
                        double dist1,dist2,dist4,dist3;
                        dist1 = dist(lines_hough2[i][j][0], lines_hough2[i][j][1], lines_hough2[i][k][0], lines_hough2[i][k][1]);
                        dist2 = dist(lines_hough2[i][j][0], lines_hough2[i][j][1], lines_hough2[i][k][2], lines_hough2[i][k][3]);
                        dist3 = dist(lines_hough2[i][j][2], lines_hough2[i][j][3], lines_hough2[i][k][0], lines_hough2[i][k][1]);
                        dist4 = dist(lines_hough2[i][j][2], lines_hough2[i][j][3], lines_hough2[i][k][2], lines_hough2[i][k][3]);
                      
                        
                        if((dist1 < 20) || (dist2 < 20 )||(dist3 < 20)||(dist4 < 20))
                        {
                            
                            l[0] = lines_hough2[i][j][0];
                            l[1] = lines_hough2[i][j][1];
                            l[2] = lines_hough2[i][j][2];
                            l[3] = lines_hough2[i][j][3];
                            vector<Vec4f>::iterator ret;
                            ret = std::find(lines_std2[i].begin(), lines_std2[i].end(), l);
                            lines_std2[count1].push_back(l);
                            
                            flag = 1;
                            break;
                        }
                    }
                }
            }
            if(flag == 1)
            {
                count1 ++;
            }
        }
        
      
        Delete_line( lines_std2, lines_std3,count1,draw);
       
        for(int i=0; i<count1;i++)
        {
            double length;
            length = dist(lines_std3[i][0][0],lines_std3[i][0][1],lines_std3[i][0][2],lines_std3[i][0][3]);
            cout << length << endl;
            if( length > 30)
            {
                line_dect.push_back(lines_std3[i][0]);
                Length.push_back(length);
            }
        }
        
        /*for(int i = 0; i < line_dect.size(); i++)
            line(draw, Point(line_dect[i][0],line_dect[i][1]),Point(line_dect[i][2],line_dect[i][3]), Scalar(0,0,255));*/
        if(line_dect.size()<1)
            Image_rect.erase(Image_rect.begin() + i);
        
    }
    return 0;
}



 
